#ifndef NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_
#define NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_

#include <memory>
#include <string>
#include <vector>

#include "nav2_core/controller.hpp"
#include "nav2_util/robot_utils.hpp"

namespace nav2_custom_controller {

    class CustomController : public nav2_core::Controller {
    public:
        CustomController() = default;
        ~CustomController() override = default;
        void configure(
            const rclcpp_lifecycle::LifecycleNode::WeakPtr &parant, std::string name,
            std::shared_ptr<tf2_ros::Buffer> tf,
            std::shared_ptr<nav2_costmap_2d::Costmap2DROS> constmap_ros) override;

        void cleanup() override;
        void activate() override;
        void deactivate() override;
        geometry_msgs::msg::TwistStamped
        computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose,
                            const geometry_msgs::msg::Twist &velocity,
                            nav2_core::GoalChecker * goal_checker) override;

        void setPlan(const nav_msgs::msg::Path &path) override;
        void setSpeedLimit(const double &speed_limit, const bool &percentage) override;

    private:
        std::string plugin_name; // 存储插件名称
        std::shared_ptr<tf2_ros::Buffer> tf_; // 存储坐标变换缓存指针，用于查询坐标变换关系
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> constmap_ros_; // 存储代价地图
        nav2_util::LifecycleNode::SharedPtr node; // 存储节点指针
        nav2_costmap_2d::Costmap2D *costmap; // 存储全局代价地图
        nav_msgs::msg::Path global_plan; // 存储 setplan 提供的全局路径
        // 最大线速度角速度
        double max_angular_speed ;
        double max_linear_speed ;

        // 获取路径中距离当前点最近的点
        geometry_msgs::msg::PoseStamped
        getNearestTargetPose(const geometry_msgs::msg::PoseStamped &current_pose);
        // 计算目标点方向和当前位置的角度差
        double calculateAngleDifference(const geometry_msgs::msg::PoseStamped &current_pose,
                                        const geometry_msgs::msg::PoseStamped &target_pose);
    };
   
}

#endif // NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_